#include "background.h"

#include <stdexcept>
#include <string.h>
#include <stdio.h>

using namespace cv;
using namespace std;

BackGround::BackGround( const int width , const int height ,
                        int channels , int pixelBytes )
{
    this->width = width;
    this->height = height;
    this->channels = channels;
    this->pixelBytes = pixelBytes;

    if( pixelBytes == 1 ) {
       bg = new uint32_t[ width*height*channels ];
       int imageType = CV_MAKETYPE(CV_8U,channels);
       bgImg = Mat::zeros(height, width, imageType);
   } else if( pixelBytes == 2 ) {
       bg = new uint32_t[ width*height*channels*2 ];
       int imageType = CV_MAKETYPE(CV_16U,channels);
       bgImg = Mat::zeros(height, width, imageType);
   } else {
       throw std::runtime_error("invalid bytes per pixel");
   }

    reset();
}

BackGround::~BackGround()
{
    if( bg != NULL ) {
        delete bg;
        bg = NULL;
    }
}

void BackGround::addImage( const cv::Mat &image )
{
    if( image.rows != height || image.cols != width )
        throw std::runtime_error("Invalid image size.");
    if( image.channels() != channels )
        throw std::runtime_error("Unexpected number of channels");
//    if( image.depth() != pixelBytes ) {
//        printf("Found pixel depth %d  expected %d\n",image.depth(),pixelBytes);
//        throw std::runtime_error("Unexpected pixel depth");
//    }

    if( pixelBytes == 1 ) {
        const uint8_t *srcPtr = (const uint8_t *)image.datastart;
        const uint8_t *srcRowPtr = srcPtr;
        uint32_t *dstPtr = bg;
        for( int i = 0; i < height; i++ ) {
            srcPtr = srcRowPtr;
            for( int j = 0; j < width; j++ ) {
                for( int k = 0; k < channels; k++ )
                    *dstPtr++ += *srcPtr++;
            }
            srcRowPtr += image.step;
        }
    } else {
        const uint16_t *srcPtr = (const uint16_t *)image.datastart;
        const uint16_t *srcRowPtr = srcPtr;
        uint32_t *dstPtr = bg;
        int srcStep = image.step/pixelBytes;
        for( int i = 0; i < height; i++ ) {
            srcPtr = srcRowPtr;

            for( int j = 0; j < width; j++ ) {
                for( int k = 0; k < channels; k++ )
                    *dstPtr++ += *srcPtr++;
            }
            srcRowPtr += srcStep;
        }
    }

    count++;
}

const cv::Mat& BackGround::getBackground()
{
    if( count == 0 )
        throw std::runtime_error("Must add an image first");

    if( pixelBytes == 1 ) {
        uint32_t *bgPtr = bg;

        uint8_t *dstPtr = (uint8_t *)bgImg.datastart;
        uint8_t *dstRowPtr = dstPtr;

        for( int i = 0; i < height; i++ ) {
            dstPtr = dstRowPtr;
            for( int j = 0; j < width; j++ ) {
                for( int k = 0; k < channels; k++ )
                    *dstPtr++ = (uint8_t)(*bgPtr++ / count);
            }
            dstRowPtr += bgImg.step;
        }
    } else {
        uint32_t *bgPtr = bg;

        uint16_t *dstPtr = (uint16_t *)bgImg.datastart;
        uint16_t *dstRowPtr = dstPtr;
        int dstStep = bgImg.step/pixelBytes;

        for( int i = 0; i < height; i++ ) {
            dstPtr = dstRowPtr;
            if( (uint64_t)dstPtr > (uint64_t)bgImg.datalimit )
                throw std::runtime_error("exceeded limit");
            for( int j = 0; j < width; j++ ) {
                for( int k = 0; k < channels; k++ )
                    *dstPtr++ = (uint16_t)(*bgPtr++ / count);
            }
            dstRowPtr += dstStep;
        }
    }

    return bgImg;
}

void BackGround::reset()
{
    memset( bg , 0 , sizeof(uint32_t)*width*height*channels*pixelBytes );
    count = 0;
}
